#include "helpers.hpp"

MatrixXd region_to_mat(Region region)
{
    auto villages = region.get_region();
    int N = villages.size();
    MatrixXd m(N, N);

    for (size_t i = 0; i < N; i++)
    {
        for (size_t j = 0; j < N; j++)
        {
            double x1 = villages[i].get_x();
            double y1 = villages[i].get_y();
            double x2 = villages[j].get_x();
            double y2 = villages[j].get_y();
            m(i, j) = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
        }
    }

    return m;
}

Graph matrix_to_graph(MatrixXd matrix)
{
    Graph graph;

    for (size_t i = 0; i < matrix.rows(); i++)
    {
        for (size_t j = 0; j < matrix.cols(); j++)
        {
            if (i == j)
                continue;

            double weight = matrix(i, j);
            int from = i;
            int to = j;

            if (graph.nodes.find(from) == graph.nodes.end())
            {
                graph.nodes[from] = new Node(from);
            }
            if (graph.nodes.find(to) == graph.nodes.end())
            {
                graph.nodes[to] = new Node(to);
            }
            Node *fromNode = graph.nodes.find(from)->second;
            Node *toNode = graph.nodes.find(to)->second;

            Edge *newEdge = new Edge(weight, fromNode, toNode);
            // 新增一条边，则to节点的入度增加
            toNode->in++;
            // from节点的出度增加
            fromNode->out++;
            fromNode->nexts.push_back(toNode);
            fromNode->edges.push_back(newEdge);
            graph.edges.insert(newEdge);
        }
    }
    return graph;
}

MatrixXd unordered_set_to_matrix(unordered_set<Edge, EdgeHash, EdgeEqual> uset)
{
    // determine the number of nodes
    std::set<int> node_ids;
    std::map<int, int> node_idx;

    for (auto ite : uset)
    {
        node_ids.insert(ite.from->value);
        node_ids.insert(ite.to->value);
    }
    int N = node_ids.size();

    MatrixXd m = MatrixXd::Zero(N, N);
    std::cout << m << std::endl;

    int idx = 0;
    for (auto ite : node_ids)
    {
        node_idx[ite] = idx++;
    }

    for (auto ite : uset)
    {
        int from = ite.from->value;
        int to = ite.to->value;

        m(node_idx[from], node_idx[to]) = ite.weight;
        m(node_idx[to], node_idx[from]) = ite.weight;

        std::cout << "from: " << from << " | to: " << to << " | weight: " << ite.weight << std::endl;
        std::cout << m << std::endl;
    }

    return m;
}

unordered_set<Edge, EdgeHash, EdgeEqual> primMST(Graph graph)
{
    // 装边的最小堆
    auto cmp = [](const Edge &e1, const Edge &e2)
    {
        return e1.weight > e2.weight;
    };
    priority_queue<Edge, vector<Edge>, decltype(cmp)> smallQueue(cmp);
    // 判断节点是否访问过
    unordered_set<Node, NodeHash, NodeEqual> node_set;
    unordered_set<Edge, EdgeHash, EdgeEqual> result;
    for (auto ite : graph.nodes)
    {
        if (node_set.find(*ite.second) == node_set.end())
        {
            // 如果没有访问,将其标记为访问过，并把它对应的边放入最小堆
            node_set.insert(*ite.second);
            for (Edge *edge : ite.second->edges)
            {
                smallQueue.push(*edge);
            }
            // 在当前这个图中寻找最小生成树
            while (!smallQueue.empty())
            {
                // 从堆中取出一个最小权重边，并取出对应节点
                Edge help_edge = smallQueue.top();
                smallQueue.pop();

                Node edge_to = *(help_edge.to);
                // 然后判断这个节点是否被访问过，如果没有则将这个边加入边集
                if (node_set.find(edge_to) == node_set.end())
                {
                    result.insert(help_edge);
                    node_set.insert(edge_to); // 标记edge_to也已经访问过了
                    for (Edge *newEdge : edge_to.edges)
                    {
                        smallQueue.push(*newEdge);
                    }
                }
            }
        }
    }
    return result;
}

//https://github.com/mburst/dijkstras-algorithm/blob/master/dijkstras.cpp
std::vector<Node> shortest_path(Graph graph, Node from, Node to)
{
    unordered_map<Node, double, NodeHash, NodeEqual> distances;
    unordered_map<Node, Node, NodeHash, NodeEqual> previous;
    std::vector<Node> nodes;
    std::vector<Node> path;

    auto comparator = [&] (Node left, Node right) { return distances[left] > distances[right]; };

    for (auto& node: graph.nodes)
    {
        if (node.first == from.value)
        {
            distances[node.first] = 0;
            // distances[*node.second] = 0;
        }
        else
        {
            distances[node.first] = std::numeric_limits<double>::max();
            // distances[*node.second] = std::numeric_limits<double>::max();
        }

        nodes.push_back(*(node.second));
        std::push_heap(std::begin(nodes), std::end(nodes), comparator);
    }
    
    for (auto &&i : nodes)
    {
        std::cout << i.value;
    }
    std::cout << std::endl;
    




    
    while (!nodes.empty())
    {
        std::pop_heap(std::begin(nodes), std::end(nodes), comparator);
        auto smallest = nodes.back();
        nodes.pop_back();

        if (smallest.value == to.value)
        {
            while (previous.find(smallest) != std::end(previous))
            {
                path.push_back(smallest);
                smallest = previous[smallest];
            }
            break;
        }
        
        if (distances[smallest] == std::numeric_limits<double>::max())
        {
            break;
        }
        
        for (size_t i = 0; i < smallest.nexts.size(); i++)
        {
            // neighbor = smallest.nexts[i]
            // edge from this to neighbor = smallest.edges[i].weight
            double alt = distances[smallest] + smallest.edges[i]->weight;
            if (alt < distances[*smallest.nexts[i]])
            {
                distances[*smallest.nexts[i]] = alt;
                previous[*smallest.nexts[i]] = smallest;
                std::make_heap(std::begin(nodes), std::end(nodes), comparator);
            }
            
        }
        
    }
    return path;
}